# -*- coding = utf-8 -*-
# @Time : 2024/8/8 8:59
# @Author : 龙王赘婿冷帝彪少
# @File : helishi.py
# @Software : PyCharm
# @Description : 和利时电机


'''
    初始最低位置： -3963000
    目标最高位置： -5563000
    向上运动为：负数
    向上运动72°，运动刻度 -1600000
    电机线数：7929856/圈

    样例报文：
    向 6040 写两个字节 0x0002，代表将电机使能 返回 60命令代表写成功
    去使能：
    0x601   2b 40 60 00 02 00 00 00
    0x581   60 40 60 00 00 00 00 00

    使能加运动
    0x601   2f 60 60 00 01 00 00 00     0x6060 写一个字节 0x01，代表配置为位置模式
    0x601   2f 19 10 00 00 00 00 00     0x1019 同步数据溢出值，这里没搞懂    快速手册里没有
    0x601   23 81 60 00 a0 86 01 00     0x6081 速度 写入四字节 0x186a0=100000 pulse/s
    0x601   23 83 60 00 20 a1 07 00     0x6083 加速度 0x7a120 pulse/s^2
    0x601   23 84 60 00 20 a1 07 00     0x6084 减速度 0x7a120 pulse/s^2
    0x601   2b 40 60 00 06 00 00 00     shutdown
    0x601   2b 40 60 00 07 00 00 00     switch on
    0x0     81 01                       刷新节点
    0x0     01 01
    0x601   2b 40 60 00 2f 00 00 00
    0x601   23 7a 60 00 48 45 b4 ff     0x607a 目标位置
    0x601   2b 40 60 00 3f 00 00 00

    # 只做更新
    0x601   2f 60 60 00 01 00 00 00     0x6060 写一个字节 0x01，代表配置为位置模式
    0x601   2f 19 10 00 00 00 00 00     0x1019 同步数据溢出值，这里没搞懂    快速手册里没有
    0x601   23 81 60 00 a0 86 01 00     0x6081 速度 写入四字节 0x186a0=100000 pulse/s
    0x601   23 83 60 00 20 a1 07 00     0x6083 加速度 0x7a120 pulse/s^2
    0x601   23 84 60 00 20 a1 07 00     0x6084 减速度 0x7a120 pulse/s^2
    0x0     81 01                       刷新节点
    0x0     01 01
    0x601   2b 40 60 00 2f 00 00 00
    0x601   23 7a 60 00 48 45 b4 ff     0x607a 目标位置
    0x601   2b 40 60 00 3f 00 00 00


    响应大同小异，不予叙述
    0x581   60 xx xx 00 00 00 00 00
    0x701   00                          这是什么响应
'''

from ctypes import *
import time
import random
import logging
from logging import handlers

# 初始化结构体
class VCI_INIT_CONFIG(Structure):
    _fields_ = [("AccCode", c_uint),
                ("AccMask", c_uint),
                ("Reserved", c_uint),
                ("Filter", c_ubyte),
                ("Timing0", c_ubyte),
                ("Timing1", c_ubyte),
                ("Mode", c_ubyte)
                ]


# 对象结构体
class VCI_CAN_OBJ(Structure):
    _fields_ = [("ID", c_uint),
                ("TimeStamp", c_uint),
                ("TimeFlag", c_ubyte),
                ("SendType", c_ubyte),
                ("RemoteFlag", c_ubyte),
                ("ExternFlag", c_ubyte),
                ("DataLen", c_ubyte),
                ("Data", c_ubyte * 8),
                ("Reserved", c_ubyte * 3)
                ]


# 接收对象结构体
class VCI_CAN_OBJ_ARRAY(Structure):
    _fields_ = [('SIZE', c_uint16),
                ('STRUCT_ARRAY', POINTER(VCI_CAN_OBJ))]  # 结构体指针

    def __init__(self, num_of_structs):
        # 将数组转换为指针
        self.STRUCT_ARRAY = cast((VCI_CAN_OBJ * num_of_structs)(), POINTER(VCI_CAN_OBJ))  # 结构体数组 这个括号不能少
        self.SIZE = num_of_structs  # 结构体长度
        self.ADDR = self.STRUCT_ARRAY[0]  # 结构体数组地址  byref()转c地址


class CAN:
    def __init__(self):
        self.lo = logging.getLogger("mylog")
        self.init_log()
        self.CAN1 = 0
        self.CAN2 = 1
        self.CanDLLName = './ControlCAN.dll'
        self.VCI_USBCAN2 = 4  # 设备类型
        self.STATUS_OK = 1
        self.canDLL = windll.LoadLibrary(self.CanDLLName)
        # canDLL = cdll.LoadLibrary('./libcontrolcan.so') # Linux
        print("can dll name is:", self.CanDLLName)

        self.NODE_ID = 3

        # 形参：类型，索引，预留
        ret = self.canDLL.VCI_OpenDevice(self.VCI_USBCAN2, 0, 0)
        if ret == self.STATUS_OK:
            self.lo.info("打开设备成功！")
        if ret != self.STATUS_OK:
            self.lo.error("打开设备失败，检查连接线 或 检查can调试助手是否占用")

        # 初始化0通道配置信息
        self.vci_initconfig = VCI_INIT_CONFIG(0x80000008,  # 验收码
                                              0xFFFFFFFF,  # 屏蔽码，表示全部接收验收码
                                              0,  # 保留
                                              0,  # 滤波方式
                                              0x00,  # 波特率定时器0
                                              0x14,  # 波特率定时器1
                                              0  # 模式 0:正常模式 1:只听模式 2:环回模式
                                              )
        # 波特率   定时器0    定时器1
        # 125K    0x03      0x1C
        # 250K    0x01      0x1C
        # 500K    0x00      0x1C
        # 1000K   0x00      0x14

    def init_can(self):
        # 参数：设备类型 设备索引 CAN通道索引（0：CAN1,1：CAN2）引用传参
        ret = self.canDLL.VCI_InitCAN(self.VCI_USBCAN2, 0, self.CAN1, byref(self.vci_initconfig))
        if ret == self.STATUS_OK:
            self.lo.info(f'配置CAN{self.CAN1+1}成功！')
        if ret != self.STATUS_OK:
            self.lo.error(f'配置CAN{self.CAN1+1}失败！')

        # 参数：设备类型 设备索引 CAN通道索引（0：CAN1,1：CAN2）
        ret = self.canDLL.VCI_StartCAN(self.VCI_USBCAN2, 0, self.CAN1)
        if ret == self.STATUS_OK:
            print(f'启动CAN{self.CAN1+1}成功!')
        if ret != self.STATUS_OK:
            self.lo.error(f'启动CAN{self.CAN1+1}失败!')

    def init_log(self):
        my_format = logging.Formatter("%(asctime)s [%(levelname)s]\t%(message)s")

        stream_handler = logging.StreamHandler()
        stream_handler.setFormatter(my_format)
        self.lo.addHandler(stream_handler)

        file_handler = handlers.TimedRotatingFileHandler(filename="motor.log", when='D')
        file_handler.setFormatter(my_format)
        self.lo.addHandler(file_handler)

        self.lo.setLevel(level=logging.INFO)

        # lo.debug("debug 调试信息")
        # lo.info("info 正常操作")
        # lo.warning("waring 告警")
        # lo.error("error 错误信息")
        # lo.critical("critical 致命错误")

    def can_send(self, addr, data) -> bool:
        ubyte_array = c_ubyte * 8
        buf = ubyte_array(data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7])
        ubyte_3array = c_ubyte * 3
        reverse = ubyte_3array(0, 0, 0)
        vci_can_obj = VCI_CAN_OBJ(addr,  # 帧地址
                                  0,   # 时间标识
                                  0,   # 是否使用时间标识
                                  1,   # 0:发送失败后4秒重发 1:单次发送
                                  0,   # 0:数据帧 1:远程帧(数据帧为空)
                                  0,   # 0:标准帧(11位ID)， 1:扩展帧(29位ID)
                                  8,   # 数据长度 (<=8)
                                  buf,    # data[8]
                                  reverse  # 系统保留
                                  )
        # 参数：设备类型 设备索引 CAN通道索引(0：CAN1,1：CAN2) 结构体包首指针 包数(建议发一包)
        ret = self.canDLL.VCI_Transmit(self.VCI_USBCAN2, 0, self.CAN1, byref(vci_can_obj), 1)
        if ret == self.STATUS_OK:
            # self.lo.info(f'send CAN{self.CAN1+1}   addr: {addr:#x}', '   data: [{}]'.format(' '.join(hex(x) for x in data)))
            return True
        if ret != self.STATUS_OK:
            self.lo.error('can send failed')
            return False

    def can_recv(self) -> [int, list]:
        recv_date = VCI_CAN_OBJ()
        # 参数：设备类型 设备索引 CAN通道索引（0：CAN1,1：CAN2）数组首指针 本次接收最大帧数 保留 返回：实际帧数
        num = self.canDLL.VCI_Receive(self.VCI_USBCAN2, 0, self.CAN1, byref(recv_date), 1, 0)
        if num > 0:
            self.lo.debug(f'recv CAN{self.CAN1 + 1}   addr: {recv_date.ID:#x}', '   data: [{}]'.format(' '.join(hex(x) for x in recv_date.Data)))
            ls = []
            for i in recv_date.Data:
                ls.append(i)
            return [recv_date.ID, ls]
        else:
            return [-1, []]

    # 通过canopen协议写入 index:写入地址 value:要写入的数据 length:该写入的长度
    def can_canopen_write(self, index, value, length) -> bool:
        # 包头封装
        node_id = self.NODE_ID
        canopen_send_addr = 0x600 + node_id

        # 包体封装
        send_buf = [0, 0, 0, 0, 0, 0, 0, 0]
        if length == 1:
            send_buf[0] = 0x2F
            send_buf[4] = (value >> 0) & 0xff
        elif length == 2:
            send_buf[0] = 0x2B
            send_buf[4] = (value >> 0) & 0xff
            send_buf[5] = (value >> 8) & 0xff
        elif length == 3:
            send_buf[0] = 0x27
            send_buf[4] = (value >> 0) & 0xff
            send_buf[5] = (value >> 8) & 0xff
            send_buf[6] = (value >> 16) & 0xff
        elif length == 4:
            send_buf[0] = 0x23
            send_buf[4] = (value >> 0) & 0xff
            send_buf[5] = (value >> 8) & 0xff
            send_buf[6] = (value >> 16) & 0xff
            send_buf[7] = (value >> 24) & 0xff
        else:
            self.lo.error("can_canopen_write send length error")
            return False

        # 目标寄存器 目标子寄存器
        send_buf[1] = index & 0xFF
        send_buf[2] = (index >> 8) & 0xFF
        send_buf[3] = 0

        # self.lo.debug(buf)
        # 发送数据
        ret = self.can_send(canopen_send_addr, send_buf)
        if ret is not True:
            self.lo.error("can_canopen_write send error")
            return False
        else:
            current_time = time.time()
            while True:
                [canopen_read_addr, recv_buf] = c.can_recv()
                if canopen_read_addr >= 0:
                    if canopen_read_addr - 0x580 == node_id and (recv_buf[1] + (recv_buf[2] << 8)) == index and \
                            recv_buf[0] == 0x60:
                        # print("ture")
                        time.sleep(0.05)
                        return True
                    else:
                        pass
                        # print("can_canopen_write receive is not right")

                if time.time() - current_time > 0.5:
                    self.lo.error("can_canopen_write wait timeout")
                    return False

    # 通过canopen协议读取某寄存器的值 欲读取的寄存器：index
    def can_canopen_read(self, index) -> [bool, int]:
        # 包头封装
        node_id = self.NODE_ID
        canopen_send_addr = 0x600 + node_id

        # 包体封装
        send_buf = [0, 0, 0, 0, 0, 0, 0, 0]

        # 命令字
        send_buf[0] = 0x40

        # 目标寄存器 目标子寄存器
        send_buf[1] = index & 0xFF
        send_buf[2] = (index >> 8) & 0xFF
        send_buf[3] = 0

        # print(buf)
        # 发送数据
        ret = self.can_send(canopen_send_addr, send_buf)
        if ret is not True:
            self.lo.error("can_canopen_read send error")
            return [False, 0]
        else:
            current_time = time.time()
            while True:
                [canopen_read_addr, recv_buf] = c.can_recv()
                if canopen_read_addr >= 0:
                    if canopen_read_addr - 0x580 == node_id and (recv_buf[1] + (recv_buf[2] << 8)) == index:
                        time.sleep(0.05)
                        if recv_buf[0] == 0x4F:
                            value = recv_buf[4]
                            return [True, value]
                        elif recv_buf[0] == 0x4B:
                            value = recv_buf[4] + (recv_buf[5] << 8)
                            return [True, value]
                        elif recv_buf[0] == 0x47:
                            value = recv_buf[4] + (recv_buf[5] << 8) + (recv_buf[6] << 16)
                            return [True, value]
                        elif recv_buf[0] == 0x43:
                            value = recv_buf[4] + (recv_buf[5] << 8) + (recv_buf[6] << 16) + (recv_buf[7] << 24)
                            return [True, value]
                        else:
                            return [False, 0]
                    else:
                        pass
                        # print("can_canopen_read receive is not right")

                if time.time() - current_time > 0.5:
                    self.lo.error("can_canopen_read wait timeout")
                    return [False, 0]

    # 获取当前位置
    def get_pos(self) -> [bool, int]:
        return self.can_canopen_read(0x6064)

    # 获取状态字
    def get_state(self) -> [bool, int]:
        return self.can_canopen_read(0x6041)

    # 设置控制寄存器 0x6040
    def set_ctrl(self, ctrl) -> bool:
        return self.can_canopen_write(0x6040, ctrl, 2)

    # 设置模式 0x1 位置模式
    def set_mode(self, mode) -> bool:
        return self.can_canopen_write(0x6060, mode, 1)

    # 设置速度 单位 cnt/s
    def set_sp(self, sp) -> bool:
        return self.can_canopen_write(0x60FF, sp, 4)

    # 设置加速度 500000 为佳
    def set_acc(self, acc) -> bool:
        if acc > 1000000:
            acc = 1000000
        return self.can_canopen_write(0x6083, acc, 4)

    # 设置减速度 500000 为佳
    def set_dcc(self, dcc) -> bool:
        if dcc > 1000000:
            dcc = 1000000
        return self.can_canopen_write(0x6084, dcc, 4)

    # 停止
    def shutdown(self) -> bool:
        return self.set_ctrl(0x06)

    # 启动
    def switch_on(self) -> bool:
        return self.set_ctrl(0x07)

    # 绝对位置模式下，准备启动
    def ready_run(self) -> bool:
        return self.set_ctrl(0x2f)

    # 绝对位置模式下，启动
    def start_run(self) -> bool:
        return self.set_ctrl(0x3f)

    def set_enable(self) -> bool:
        return self.set_ctrl(0x0f)

    def set_disable(self) -> bool:
        return self.set_ctrl(0x00)

    def set_goal_pos(self, pos) -> bool:
        return self.can_canopen_write(0x607a, pos, 4)

    def speed_move(self):
        # 配置为速度模式
        self.set_mode(0x3)
        # 使能
        self.set_enable()
        # 配置速度
        speed = 1200  # 电机 单位 r/min

        # 谐波减速机的减速比是30
        CNT = 4096
        sp = int(CNT * speed / 60)  # 单位 r/s
        print("speed: ", speed, "r/min")
        self.set_sp(sp)

    def speed_stop(self):
        # 去使能
        self.set_disable()
    def move_pos(self, pp: int, sp: int):
        # 配置为位置模式
        self.set_mode(0x1)
        # 配置速度
        self.set_sp(sp)
        # 配置加速度
        self.set_acc(500000)
        # self.set_acc(50000)
        # 配置减速度
        self.set_dcc(500000)
        # self.set_dcc(50000)
        # 准备移动
        self.ready_run()
        # 配置位置
        self.set_goal_pos(pp)
        # 开始移动
        self.start_run()

    def move_pos_low(self, pp: int):
        self.move_pos(pp, 200000)

    def move_pos_fast(self, pp: int):
        self.move_pos(pp, 300000)


if __name__ == "__main__":
    # 创建对象
    c = CAN()

    # 初始化can口
    c.init_can()

    while True:
        while True:
            key = input("请输入")  # 输入指令
            if key == "move":
                c.speed_move()
            elif key == "stop":
                c.speed_stop()
            else:
                pass




